#include "teleopwindow.h"
#include "iostream"
#include <stdio.h>
#include <stdlib.h>

using namespace std;

TeleopWindow::TeleopWindow()
{
	char buffer[20];
  set_title("Teleop");
  set_border_width(10);
  add(m_container);

  velocidadTraslacional=0.0f;
  velocidadRotacional = 0.0f;
  aceleracionTraslacional = 1.2f;
  aceleracionRotacional = 0.3f;
  velocidadTraslacionalMaxima = 15.0f;
  velocidadRotacionalMaxima = 5.0f;

  m_container.set_row_spacing(10);

  m_container.set_column_spacing(10);

  lbVelocidadTraslacional.set_label("Velocidad traslacional");

  lbVelocidadRotacional.set_label("Velocidad rotacional");

  lbAceleracionTraslacional.set_label("Aceleracion traslacional");

  lbAceleracionRotacional.set_label("Aceleracion rotacional");

  lbVelocidadTraslacionalMaxima.set_label("Velocidad traslacional maxima");

  lbVelocidadRotacionalMaxima.set_label("Velocidad rotacional maxima");

  txtVelocidadTraslacional.set_size_request(150,20);

  txtVelocidadRotacional.set_size_request(150,20);

  txtAceleracionTraslacional.set_size_request(150,20);

  txtAceleracionRotacional.set_size_request(150,20);

  txtVelocidadRotacionalMaxima.set_size_request(150,20);

  txtVelocidadTraslacionalMaxima.set_size_request(150,20);

  // Main Container:
  m_container.attach(lbVelocidadTraslacional,1,1,1,1);
  m_container.attach(lbVelocidadRotacional,1,2,1,1);
  m_container.attach(lbAceleracionTraslacional,1,3,1,1);
  m_container.attach(lbAceleracionRotacional,1,4,1,1);
  m_container.attach(lbVelocidadTraslacionalMaxima,1,5,1,1);
  m_container.attach(lbVelocidadRotacionalMaxima,1,6,1,1);
  m_container.attach(txtVelocidadTraslacional,2,1,1,1);
  m_container.attach(txtVelocidadRotacional,2,2,1,1);
  m_container.attach(txtAceleracionTraslacional,2,3,1,1);
  m_container.attach(txtAceleracionRotacional,2,4,1,1);
  m_container.attach(txtVelocidadTraslacionalMaxima,2,5,1,1);
  m_container.attach(txtVelocidadRotacionalMaxima,2,6,1,1);

  // Events.
  // We override the default event signal handler.
  add_events(Gdk::KEY_PRESS_MASK | Gdk::KEY_RELEASE_MASK);

  show_all_children();

  pressed[UP] = false;
  pressed[DOWN] = false;
  pressed[LEFT] = false;
  pressed[RIGHT] = false;

  sprintf(buffer,"%f",velocidadRotacionalMaxima);
  txtVelocidadRotacionalMaxima.set_label(buffer);
  sprintf(buffer,"%f",velocidadTraslacionalMaxima);
  txtVelocidadTraslacionalMaxima.set_label(buffer);
  sprintf(buffer,"%f",aceleracionTraslacional);
  txtAceleracionTraslacional.set_label(buffer);
  sprintf(buffer,"%f",aceleracionRotacional);
  txtAceleracionRotacional.set_label(buffer);

  updateThread = Glib::Threads::Thread::create(
  sigc::mem_fun(this, &TeleopWindow::update));
  updateThread->join();
}

bool TeleopWindow::on_key_release_event(GdkEventKey* event){
	if(event->keyval == GDK_KEY_Up){
		pressed[UP] = false;
	  }else if(event->keyval == GDK_KEY_Down){
		pressed[DOWN] = false;
	  }else if(event->keyval == GDK_KEY_Left){
		pressed[LEFT] = false;
	  }else if(event->keyval == GDK_KEY_Right){
		pressed[RIGHT] = false;
	  }

	  //if the event has not been handled, call the base class
	  return Gtk::Window::on_key_release_event(event);
}

bool TeleopWindow::on_key_press_event(GdkEventKey* event)
{
	if(event->keyval == GDK_KEY_Escape)
  {
    hide();
    return true;
  }else if(event->keyval == GDK_KEY_Up){
	  pressed[UP] = true;
  }else if(event->keyval == GDK_KEY_Down){
	  pressed[DOWN] = true;
  }else if(event->keyval == GDK_KEY_Left){
	  pressed[LEFT] = true;
  }else if(event->keyval == GDK_KEY_Right){
	  pressed[RIGHT] = true;
  }
  //if the event has not been handled, call the base class
  return Gtk::Window::on_key_press_event(event);
}

TeleopWindow::~TeleopWindow()
{
}

void TeleopWindow::update()
{
	char bufferT[100],bufferR[100];
	if(pressed[UP]){
		velocidadTraslacional+=aceleracionTraslacional/2;
	}
	if(pressed[DOWN])
		velocidadTraslacional-=aceleracionTraslacional/2;
	if(pressed[RIGHT])
		velocidadRotacional+=aceleracionRotacional/2;
	if(pressed[LEFT])
		velocidadRotacional+=aceleracionRotacional/2;

	if(velocidadTraslacional > velocidadTraslacionalMaxima)
		velocidadTraslacional = velocidadTraslacionalMaxima;
	else if(velocidadTraslacional < -velocidadTraslacionalMaxima)
		velocidadTraslacional = -velocidadTraslacionalMaxima;

	if(velocidadRotacional > velocidadRotacionalMaxima)
		velocidadRotacional = velocidadRotacionalMaxima;
	else if(velocidadRotacional < -velocidadRotacionalMaxima)
		velocidadRotacional = -velocidadRotacionalMaxima;

	sprintf(bufferT,"%f",velocidadTraslacional);
	txtVelocidadTraslacional.set_label(bufferT);
	sprintf(bufferR,"%f",velocidadRotacional);
	txtVelocidadRotacional.set_label(bufferR);


	Glib::usleep(1000000L);
	updateThread = Glib::Threads::Thread::create(
			sigc::mem_fun(this, &TeleopWindow::update));
	cout<<"Update"<< endl;
}

